package edu.gatech.cc.liam.geometry.linear;

import java.util.ArrayList;

import edu.gatech.cc.liam.geometry.linear.convexhull.Preferences;
import edu.gatech.cc.liam.geometry.linear.convexhull.QHalf;

public class ApproximateEuclideanBall extends ArrayList<NPoint> {

	private static final long serialVersionUID = 5070284783150382942L;
	
	private static final double TOLERANCE = Preferences.TOLERANCE;
	
	/**
	 * creates points evenly spaced on a Euclidean ball such that
	 * the distance between nearest points is less than maxDistance.
	 * In other words, every point will have at least dimensions-1 points closer than maxDistance to it.
	 * @param dimensions
	 * the dimensions of the hypershpere
	 * @param maxDistance 
	 * the maxmimum angle in radians between points
	 */
	public ApproximateEuclideanBall(int dimensions, double maxDistance) {
		super();
		NPoint startLocation = new NPoint(new double[dimensions]);
		addShell(startLocation, maxDistance, 1.0, dimensions, dimensions);
		//double[] rotations = new double[dimensions-1];
		//for()
	}

	public ApproximateEuclideanBall(ArrayList<NPoint> norms) {
		super(norms);
	}
	
	private void addShell(NPoint centerLocation, double maxDist, double radius, int dimension, int origDim) {
		assert dimension >= 0;
		assert radius >= -TOLERANCE;
		assert maxDist > 0;
		if(radius <= TOLERANCE) {
			NPoint pt = centerLocation.clone();
			//pt.values[0] = radius;
			this.add(pt);
			assert (pt.distance(new NPoint(new double[pt.values.length]))-1.0) < TOLERANCE;
		} else if(dimension == 0) {
			// do nothing
		} else {
			double angleToRot = getEvenAngleToRotate(maxDist, radius, origDim, Math.PI);
			for(double curAngle = 0; curAngle <= Math.PI + TOLERANCE; curAngle += angleToRot) {
				NPoint pt = centerLocation.clone();
				pt.values[dimension-1] = radius * Math.cos(curAngle);
				double newRadius = radius * Math.sin(curAngle);
				addShell(pt, maxDist, newRadius, dimension-1, origDim);
			}
		}
	}
	
/*	private void addOrthantShell(NPoint centerLocation, double maxDist, double radius, int dimension, int origDim) {
		assert dimension >= 0;
		assert radius >= -tolerance;
		assert maxDist > 0;
		if(radius <= tolerance) {
			NPoint pt = centerLocation.clone();
			//pt.values[0] = radius;
			this.add(pt);
			assert (pt.distance(new NPoint(new double[pt.values.length]))-1.0) < tolerance;
		} else if(dimension == 0) {
			// do nothing
		} else {
			double angleToRot = getEvenAngleToRotate(maxDist, radius, origDim, Math.PI);
			for(double curAngle = 0; curAngle <= Math.PI/2 + tolerance; curAngle += angleToRot) {
				NPoint pt = centerLocation.clone();
				pt.values[dimension-1] = radius * Math.cos(curAngle);
				double newRadius = radius * Math.sin(curAngle);
				addOrthantShell(pt, maxDist, newRadius, dimension-1, origDim);
			}
		}
	}*/
	
	public static int getMaxNumPointsCoverPerD(double approxError, int dimensions) {
		return 2 * (int)(Math.PI + TOLERANCE / getEvenAngleToRotate(approxError, 1.0, dimensions, Math.PI));
	}
	
	private static double getEvenAngleToRotate(double maxDist, double radius, int dimension, double angleToCover) {
		double maxAngle = getAngleToRotate(maxDist, radius, dimension);
		return evenAngle(maxAngle, angleToCover);
	}
	private static double evenAngle(double maxAngle, double angleToCover) {
		int numTimesToRotate = (int) Math.ceil(angleToCover / maxAngle);
		double evenAngle = angleToCover / numTimesToRotate;
		assert(evenAngle < maxAngle + TOLERANCE);
		return evenAngle;
	}
	
	private static double getAngleToRotate(double maxDist, double radius, int dimension) {
		assert dimension > 0;
		if(dimension == 1)
			return Math.PI;
		double angle = radius / (maxDist + radius);
		angle = Math.pow(angle, 1.0/(dimension-1.0));
		angle = 2 * Math.acos(angle);
		return angle;
	}
	
	private void verifyBallCovering(int dimensions, double maxDistance) {
		for(NPoint aP : this) {
			int numPtsWithin = numPtsWithin(aP, maxDistance);
			System.out.println("PT:" + aP + " within range:" + numPtsWithin);
			if(numPtsWithin < dimensions+1)
				System.out.println("UH OH, too few POINTS!!");
		}
	}
	private int numPtsWithin(NPoint thePt, double maxDistance) {
		int count = 0;
		for(NPoint aP : this) {
			if (thePt.distance(aP) <= maxDistance)
				count ++;
		}
		return count;
	}
	
	@Override
	public String toString() {
		return this.size() + " pts: " + super.toString();
	}
	
	private static void testBall(int dimensions, double maxDistance) {
		ApproximateEuclideanBall testBall = new ApproximateEuclideanBall(dimensions, maxDistance);
		System.out.print("TestBall( " + dimensions + ", " + maxDistance + " ): ");
		if(testBall.size() < 20)
			System.out.print(testBall);
		else
			System.out.print(testBall.size() + " hyperplanes");
		Polytope testHull = new Polytope(testBall, -1.0);
		PointSet verts = QHalf.qHalfspaceHull(testHull);
		System.out.println( " :: " + verts.size() + " vertices");
		NPoint orig = new NPoint(new double[testHull.getDimensions()]);
		double maxDist = -1.0;
		double minDist = Double.MAX_VALUE;
		double avgDist = 0.0;
		for(NPoint p : verts) {
			double distance = p.distance(orig);
			maxDist = Math.max(maxDist, distance);
			minDist = Math.min(minDist, distance);
			avgDist += distance;
			//System.out.println("Pt: " + p + " Distance to O: " + distance);
		}
		avgDist /= verts.size();
		System.out.println( ((maxDist-1.0)<maxDistance+TOLERANCE ? "PASSED" : "**FAILED**") + "  Max Distance from sphere: " + (maxDist-1.0) + "  Min Dist: " + (minDist-1.0) + "  Average Dist: " + (avgDist-1.0) );
	}
	
	public static ArrayList<NPoint> getHyperCube(int dimensions) {
		ArrayList<NPoint> rList = new ArrayList<NPoint>();
		NPoint origin = new NPoint(dimensions, 0.0);
		for(int d=0; d<dimensions; d++) {
			NPoint up = origin.clone();
			up.values[d] = 1.0;
			rList.add(up);
			NPoint down = origin.clone();
			down.values[d] = -1.0;
			rList.add(down);
		}
		return rList;
	}
	
	/**
	 * 
	 * @param orthant
	 * Array of booleans depicting if that dimension is positive (true) or negative (false).
	 * @return
	 */
	public ArrayList<NPoint> getVectorsInOrthant(boolean[] orthant){
		ArrayList<NPoint> rPoints = new ArrayList<NPoint>();
		for(NPoint p : this) {
			if(isInOrthant(orthant, p))
				rPoints.add(p);
		}
		return rPoints;
	}
	private static boolean isInOrthant(boolean[] orthant, NPoint p) {
		assert orthant.length == p.values.length;
		for(int d=0; d<orthant.length; d++) {
			if(orthant[d]) {
				if( p.values[d] < 0.0 )
					return false;
			} else {
				if( p.values[d] > 0.0)
					return false;
			}		
		}
		return true;
	}
	
	
	public static void main(String[] args) {
		testBall(2, 0.001);
		System.exit(0);
		testBall(4, 0.1);
		testBall(3, 0.005);
		testBall(3, 0.0025);
		testBall(3, 0.001);
		System.exit(0);
//		int dims[] = {2,3,4,5,10,20};
//		for(int i=0; i<dims.length; i++) {
//			System.out.print("Dim " + dims[i] + " \t max angle: " + getAngleToRotate(0.01, 1.0, dims[i]));
//			System.out.println("\t  Dim " + dims[i] + " \t perfect angle: " + getEvenAngleToRotate(0.01, 1.0, dims[i], Math.PI*2));
//		}
//		System.out.println();
		testBall(5, Math.sqrt(5)-1);
		testBall(2, Math.sqrt(2)-1); // square
		testBall(2, 0.158512668); // octagon
		testBall(2, 10);
		testBall(3, 1.23205081); // cube?
		testBall(3, 0.1);
		testBall(4, 0.1);
		testBall(5, 0.1);
		testBall(5, 0.2);
		testBall(5, 0.3);
		testBall(5, 0.5);
		testBall(5, 0.9);
		testBall(6, Math.sqrt(6)-1);
		testBall(7, Math.sqrt(7)-1);
		testBall(2, 0.01);
		testBall(2, 0.03);
		testBall(2, 0.07);
		testBall(2, 0.002);
		
//		double angleToRot = getAngleToRotate(0.01, 1.0, 3, Math.PI * 2);
//		System.out.println("Orig Max Angle: " + angleToRot);
//		for(double curAngle = 0; curAngle < Math.PI*2-0.0000001; curAngle += angleToRot) {
//			newAngleToRot = getAngleToRotate(0.01, )
//			System.out.println("After angle: " + curAngle + "\t newAngleToRot: " + )
//		}
	}
}
